// Copyright(c) 2011 Matthew McMullan, Jorel Lalicki and Mike Heise, Jorel Lalicki and Mike Heise. See copying.txt for details.

#ifndef __agml_cos_h__
#define __agml_cos_h__

// make sure that this file is never included directly
#ifndef __agml_agml_h__
#error "Include AGML from agml.h only. This file is not a valid entry point for the library."
#endif

#include <math.h>   /*  fabsf(3)  */

namespace agml {

/*! parabolic fast cosine approximation with good error bounds  */
inline f32 cos(f32 theta) {
    return sin(theta + (f32)_pi_2);
}
/*!	faster cos approximation with domain restricted to [-pi, pi]	*/
inline f32 _cos(f32 theta) {
	theta += (f32)_pi_2;
	return _sin( (theta > (f32)_pi) ? (theta - (f32)_2pi) : (theta));
}

/*!	SIMD cosine with restricted domain	*/
inline void _cos(_v128 &theta) {
	static const _v128 pi_over_2 = {(f32)_pi_2,(f32)_pi_2,(f32)_pi_2,(f32)_pi_2};
	static const _v128 wrapping_factor = {-(f32)_pi,-(f32)_pi,-(f32)_pi,-(f32)_pi};
	static const _v128 extra_factor = {(f32)_2pi,(f32)_2pi,(f32)_2pi,(f32)_2pi};
	theta = _mm_sub_ps(theta,pi_over_2);
	_v128 tmp = _mm_cmplt_ps(theta,wrapping_factor);
	tmp = _mm_and_ps(tmp,extra_factor);
	theta = _mm_add_ps(theta,tmp);
	_sin(theta);
}

/*! fast secant approximation with good error bounds    */
inline f32 sec(f32 theta) {
    return 1.0f/cos(theta);
}

/*!	simdified fast secant approx	*/
inline void sec(_v128& thetas) {
	_cos(thetas);
	_mm_rcp_ps(thetas);
}

/*! polynomial fast arccos approximation with good error bounds until within
 * ~3% of domain edge  */
inline f32 acos(f32 x) {
		return (f32)_pi_2 - asin(x);
}

/*!	simd arccosine	*/
inline void acos(_v128 &x) {
	static const _v128 _pis = {(f32)_pi_2, (f32)_pi_2, (f32)_pi/2, (f32)_pi_2};
	asin(x);
	x = _mm_sub_ps(_pis, x);
}

}
#endif
